#include <Arduino.h>
#include "FastAccelStepper.h"
#include <EEPROM.h>
#include <WiFi.h>
#include <ESPmDNS.h>

#include <SPIFFS.h>

//#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <AsyncElegantOTA.h>

#define MQTT_MAX_PACKET_SIZE 512
#include <PubSubClient.h>
#include <ArduinoJson.h>




#define JSJ_CTRLR_ID 1 //卷帘机控制器ID  1~3

const char *ssid = "xjaujsj";
const char *password = "1223334444";

// IP各段的值
#define IP1 192
#define IP2 168
#define IP3 1
#define IP4 1

//传感器静态地址设置为100+传感器序号，比如.101
IPAddress staticIP(IP1, IP2, IP3, 110 + JSJ_CTRLR_ID); //卷帘机控制器 IP = 110+JSJ_CTRLR_ID
IPAddress gateway(IP1, IP2, IP3, IP4);                 //默认网关
IPAddress subnet(255, 255, 255, 0);                    //子网掩码
IPAddress dns(IP1, IP2, IP3, IP4);                     // DNS

AsyncWebServer server(80);



//步进电机驱动
#define x_pul 15
#define x_dir 2
#define y_pul 4
#define y_dir 16
#define z_pul 17
#define z_dir 5
// 限位器
#define MIN_X 34
#define MIN_Y 35
#define MIN_Z 32

#define MAX_X 33
#define MAX_Y 25
#define MAX_Z 26

#define X_MAX_MOV 720
#define Y_MAX_MOV 563
#define Z_MAX_MOV 365



#define MIN_STEP_uS 5 // 最小补偿脉冲宽度，步进电机驱动器最高频率 200KHZ,脉宽最小5uS
#define X_ACCELERATE 2000
#define Y_ACCELERATE 2000
#define Z_ACCELERATE 2000
//----------------------------------------

#define TX_PIN 21 // Rx pin which the S8 Tx pin is attached to (change if it is needed)
#define RX_PIN 22 // Tx pin which the S8 Rx pin is attached to (change if it is needed)
//---------EEPROM 地址分区映射----------

#define EEPROM_SIZE 512      // 存储和恢复数据点，防止重启后执行器状态变更
#define WIFI_CON_MAX_TRY 50  // WIFI最大尝试连接次数
#define MQTT_CON_MAX_TRY 5   // MQTT最大尝试连接次数
#define FW_VERSION 1.2       // 程序版本号，用于升级标识
#define MQTT_UPLOAD_CYCLE 30 // mqtt 主动上报周期:秒

//----------------------------------------
typedef struct dp_tag
{
  int32_t x; //
  int32_t y; //
  int32_t z; //

  int32_t delta_x; //
  int32_t delta_y; //
  int32_t delta_z; //

  uint16_t rssi; // RSSI
} dev_data_t;

//------------------------------------------------------------------
FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *x_stepper = NULL;
FastAccelStepper *y_stepper = NULL;
FastAccelStepper *z_stepper = NULL;

dev_data_t x_devdatapoint; // 数据点



// String device_rec_topic = JXB_CtrlerNO1_ID + "/+" ;
// const char *userName = "562627";    // oneNET产品ID
// const char *passWord = "farmbotv1"; // oneNET鉴权信息
// String deviceID = "1158863678";     // oneNET设备ID

// // Add your MQTT Broker IP address
// const char *mqtt_server = "mqtt.heclouds.com";
// const uint16_t mqtt_port = 6002;

// EMQX MQTT Broker,本地服务器（EMQX）
const char *mqtt_server = "192.168.182.75";
const char *topic = "esp32/test";
const char *mqtt_username = "xjaujsj";
const char *mqtt_password = "xjaujsj2204";
String deviceID = "esp32-client-test1";
const int mqtt_port = 1883;

WiFiClient espClient;
PubSubClient client(espClient);
String uploadJson; // mqtt publish payload
// AsyncWebServer server(80);

//------------IPC线程通信对象-----------
typedef struct msg_q_tag
{
  char cmd[10];     // 设备控制命令
  int32_t v1;       // 控制命令参数1
  int32_t v2;       // 控制命令参数2
  int32_t v3;       // 控制命令参数3
} ctrl_msg_queue_t; // 接收到的MOSQUITTO命令解析后发往执行线程的MSG Q

xQueueHandle g_mqtt_ctrl_msgQ; // tlink下发控制命令消息队列句柄

//------TASK HANDLER-------
TaskHandle_t mqttTaskHandle = NULL;        // mqtt keyservice tsk handle
TaskHandle_t mqttExcutorTaskHandle = NULL; // mqtt cmd executor tsk handle

uint8_t dataPoint_EPPROM_shadowReg[EEPROM_SIZE]; // 创建要放入EEPROM的数组，用于memecp 到 x_devdatapoint数据点



void http_rec_callback(String cmd_typ,int16_t x,int16_t y,int16_t z);
//-------------------------------------------------------------------
//++++++++++++++++++++++++++++ static function define ++++++++++++++++++++++++++++++++++
//----函数声明-----
void system_software_reset(void);
void mqtt_upload_json(void);
void move2orgin_p();
// void clt_x_num();
// void clt_y_num();
// void clt_z_num();
/********************************************************
* Function name: setup_wifi
* Description  : poll for modbus holding reg data
* Parameter    : void

* return        void
**********************************************************/
static void setup_wifi(void)
{

  static int8_t connect_cnt = 30; //控制连接超时时长30秒
  WiFi.mode(WIFI_STA);
  //传入参数静态IP地址,网关.DNS1
  if (WiFi.config(staticIP, gateway, subnet, dns, dns) == false)
  {
    Serial.println("static IP configuration failed.");
  }

  WiFi.begin(ssid, password); //连接WIFI热点
  Serial.println("正在连接WIFI热点>>>");

  while (WiFi.status() != WL_CONNECTED && (--connect_cnt) > 0)
  {
    Serial.print(".");
    delay(1000);
  }
  if (connect_cnt < 1) //连接超时，重启
  {
    ESP.restart(); //重启ESP32
  }
  Serial.print("\nIP位址：");
  Serial.println(WiFi.localIP());

}

void http_server_init(){


  // URL与文件系统目录声明
  server.serveStatic("/", SPIFFS, "/www/").setDefaultFile("index.html");
  server.serveStatic("/favicon.ico", SPIFFS, "/www/favicon.ico");
  server.serveStatic("/jquery-1.12.1.min.js", SPIFFS, "/www/jquery-1.12.1.min.js");
  server.serveStatic("/jquery-ui.css", SPIFFS, "/www/jquery-ui.css");
  server.serveStatic("/jquery-ui.min.js", SPIFFS, "/www/jquery-ui.min.js");
  server.serveStatic("/bootstrap.min.css", SPIFFS, "/www/bootstrap.min.css");

  server.on("/ctrl", HTTP_GET, [](AsyncWebServerRequest *request)
            {
    if (request->hasParam("dir")) {
      AsyncWebParameter* p = request->getParam("dir");
      if (p->value() == "GOUP") {
        http_rec_callback("RMOV",10,10,10);
        // delay(1000);      //防止接触器换向短路
      } else if (p->value() == "STOP") {
        http_rec_callback("STOP",0,0,0);
        // delay(1000);      //防止接触器换向短路
      } else if (p->value() == "DOWN"){
        http_rec_callback("RMOV",-10,-10,-10);
        // delay(1000); //防止接触器换向短路
      }
    }//END OF if (request->hasParam("dir"))

  request->send(200, "text/plain", "OK!"); });

  server.on("/mov", HTTP_GET, [](AsyncWebServerRequest *request)
            {
    if (request->hasParam("X")) {
      AsyncWebParameter* p = request->getParam("X");
      uint16_t move_step = p->value().toInt();
      if ( move_step != 0) {
        http_rec_callback("RMOV",move_step,0,0);
        // delay(1000);      //防止接触器换向短路
      }
    }//END OF if (request->hasParam("dir"))
    if (request->hasParam("Y")) {
      AsyncWebParameter* p = request->getParam("Y");
      uint16_t move_step = p->value().toInt();
      if ( move_step != 0) {
        http_rec_callback("RMOV",0,move_step,0);
        // delay(1000);      //防止接触器换向短路
      }
    }//END OF if (request->hasParam("dir"))
    if (request->hasParam("Z")) {
      AsyncWebParameter* p = request->getParam("Z");
      uint16_t move_step = p->value().toInt();
      if ( move_step != 0) {
        http_rec_callback("RMOV",0,0,move_step);
        // delay(1000);      //防止接触器换向短路
      }
    }//END OF if (request->hasParam("dir"))

  request->send(200, "text/plain", "OK!"); });



  server.begin();
  Serial.println("卷帘机控制器初始化完成！");
}


/********************************************************
* Function name: OTA_webserver_init
* Description  : AsyncElegantOTA 升级初始化，创建两个路由：
                 /根目录展示json
                 /update 升级fermware
* Parameter    : void

* return        void
**********************************************************/
void OTA_webserver_init(void)
{
  server.on("/", HTTP_GET, [](AsyncWebServerRequest *request)
            { request->send(200, "text/plain", uploadJson.c_str()); });

  AsyncElegantOTA.begin(&server); // Start ElegantOTA
  server.begin();
  Serial.println("HTTP server started, OTA server started");
}

/********************************************************
 * Function name: callback
 * Description  : MQTT 接收消息回调函数
 * Parameter    :
 * @topic:      传感器节点收到的 mqtt topic
 * @message:    传感器节点收到的 mqtt message payload
 * @length:     payload长度
 * return        void
 **********************************************************/
void mqtt_rec_callback(char *topic, byte *message, unsigned int length)
{

  ctrl_msg_queue_t mqtt_ctrl_msg; // 接收到的MOSQUITTO控制消息，包含key:读写标识 和 value:控制量
  String mqttTopic(topic);
  String messageJson(message, length);                               // mqtt message from server,字符数组构造字符串对象
  Serial.printf("\r\nmqtt topic: %s\r\n", mqttTopic.c_str());        // print message
  Serial.printf("\r\nMessage payload: %s\r\n", messageJson.c_str()); // print message

  // String input;

  StaticJsonDocument<256> doc;

  DeserializationError error = deserializeJson(doc, messageJson);

  if (error)
  {
    Serial.print("deserializeJson() failed: ");
    Serial.println(error.c_str());
    return;
  }

  long x = doc["x"];                              // -1
  long y = doc["y"];                              // -1
  long z = doc["z"];                              // -1
  const char *cmd = doc["cmd"];                   // "AMOV"
  memcpy(mqtt_ctrl_msg.cmd, cmd, sizeof("CMDT")); // 命令长度固定为4byte


  x_devdatapoint.delta_x=x-x_devdatapoint.x;
  x_devdatapoint.delta_y=y-x_devdatapoint.y;
  x_devdatapoint.delta_z=z-x_devdatapoint.z;

  mqtt_ctrl_msg.v1 = x_devdatapoint.delta_x;
  mqtt_ctrl_msg.v2 = x_devdatapoint.delta_y;
  mqtt_ctrl_msg.v3 = x_devdatapoint.delta_z;

  x_devdatapoint.x=x;
  x_devdatapoint.y=y;
  x_devdatapoint.z=z;
  if(mqtt_ctrl_msg.cmd=="ZERO")
  {
    x_devdatapoint.x=0;
    x_devdatapoint.y=0;
    x_devdatapoint.z=0;
    x_devdatapoint.delta_x=0;
    x_devdatapoint.delta_y=0;
    x_devdatapoint.delta_z=0;
    mqtt_ctrl_msg.v1 = 0;
    mqtt_ctrl_msg.v2 = 0;
    mqtt_ctrl_msg.v3 = 0;
    move2orgin_p();
  }
  else if (xQueueSend(g_mqtt_ctrl_msgQ, &mqtt_ctrl_msg, 2000) == pdPASS)
  {
    Serial.println("mqtt cmd send to execute thread");
  }

  mqtt_upload_json(); // 通过数据下发功能，可触发callback的 ACK，达到按照用户需求实时刷新目的
}



/********************************************************
 * Function name: callback
 * Description  : MQTT 接收消息回调函数
 * Parameter    :
 * @topic:      传感器节点收到的 mqtt topic
 * @message:    传感器节点收到的 mqtt message payload
 * @length:     payload长度
 * return        void
 **********************************************************/
void http_rec_callback(String cmd_typ,int16_t x,int16_t y,int16_t z)
{

  ctrl_msg_queue_t mqtt_ctrl_msg; // 接收到的MOSQUITTO控制消息，包含key:读写标识 和 value:控制量

  memcpy(mqtt_ctrl_msg.cmd, cmd_typ.c_str(), sizeof("CMDT")); // 命令长度固定为4byte



  mqtt_ctrl_msg.v1 = x;
  mqtt_ctrl_msg.v2 = y;
  mqtt_ctrl_msg.v3 = z;

   if (xQueueSend(g_mqtt_ctrl_msgQ, &mqtt_ctrl_msg, 2000) == pdPASS)
  {
    Serial.println("mqtt cmd send to execute thread\r\n");
  }

  // mqtt_upload_json(); // 通过数据下发功能，可触发callback的 ACK，达到按照用户需求实时刷新目的
}
/********************************************************
* Function name: mqtt_reconnect
* Description  : 重新连接MOSQUITTO 服务器，超过最大连接数后重启ESP32
* Parameter    : void

* return        void
**********************************************************/
void mqtt_reconnect(void)
{
  uint8_t recon_counter = MQTT_CON_MAX_TRY; // 最大重连5次
  client.setServer(mqtt_server, mqtt_port);
  client.setCallback(mqtt_rec_callback);
  // Loop until we're reconnected
  while (!client.connected())
  {
    if (--recon_counter < 1)
    {
      Serial.println("reconnect failed after 5 times of trying, reset ESP32...");
      system_software_reset(); // save datapoint and reset
    }

    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    deviceID += String(WiFi.macAddress());
    // if (client.connect(deviceID.c_str(), userName, passWord))
    if (client.connect(deviceID.c_str(), mqtt_username, mqtt_password))
    {
      Serial.println("device connected");
      client.publish(topic, "Hi EMQX I'm ESP32 ^^");
      client.subscribe(topic);
    }
    else
    {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}

/********************************************************
* Function name: mqtt_connect
* Description  : connect to mqtt broker
* Parameter    : void

* return        void
**********************************************************/
static void mqtt_connect(void)
{
  client.setServer(mqtt_server, mqtt_port);
  client.setCallback(mqtt_rec_callback);
  mqtt_reconnect(); //
}

/********************************************************
* Function name: mqtt_upload_json
* Description  : 获取RSSI，通过string的拼接，构造mqtt json 数据
* Parameter    : void

* return        void
**********************************************************/
static void datapoint_to_json(void)
{
  //-------通过修改topic的末尾数据，可实现连续多个设备的数据转发上报功能，是数据网关的关键功能！

  //---------UPDATE RELAY STATE----------
  uploadJson.clear();
  uploadJson = "{\"x\":";
  uploadJson += x_devdatapoint.x;
  uploadJson += ",\"y\":";
  uploadJson += x_devdatapoint.y;
  uploadJson += ",\"z\":";
  uploadJson += x_devdatapoint.z;
  uploadJson += ",\"rssi\":";
  uploadJson += x_devdatapoint.rssi;
  uploadJson += "}";

  Serial.println(uploadJson.c_str());
}

/********************************************************
* Function name: mqtt_upload_json
* Description  : 获取RSSI，通过string的拼接，构造mqtt json 数据
* Parameter    : void

* return        void
**********************************************************/
void mqtt_upload_json(void)
{
  static uint8_t mqttUploadCnt = 10;
  datapoint_to_json(); // datapoint 对象，拼接成json，结果存在uploadJson全局字符串中

  // bool pubRes = client.publish(JXB_CtrlerNO1_ID.c_str(),uploadJson.c_str());      //发布json数据
  bool pubRes = client.publish(uploadJson.c_str(), uploadJson.c_str()); // 发布json数据
  while (mqttUploadCnt > 1)
  {
    mqttUploadCnt--;
    // system_software_reset();
  }
  if (pubRes != false)
  {
    mqttUploadCnt = 10;
  }
}

/********************************************************
* Function name: tsk_mqt_mqtt_keepalive
* Description  : 连接并保活MOSQUITTO MQTT服务器连接
* Parameter    : pvParam 线程创建传入参数


* return         void
**********************************************************/
void tsk_mqtt_service(void *pvParam)
{
  mqtt_connect(); // 连接MOSQUITTO

  delay(3000);        //
  mqtt_upload_json(); // upload json mqtt data 第一次设备上线上报数据
  while (1)
  {
    mqtt_reconnect(); // 连接tlink server
    client.loop();    // MQTT消息处理函数
    delay(100);
  }
}
/********************************************************
* Function name: steper_motor_init
* Description  : 初始化fastAccelSteper步进电机控制对象
* Parameter    : void

* return         void
**********************************************************/
void steper_motor_init(void)
{
  //-----行程开关IO初始化----------
  pinMode(MIN_X, INPUT_PULLUP); // X轴最小行程限位，同时也是归零点
  pinMode(MIN_Y, INPUT_PULLUP); // Y轴最小行程限位，同时也是归零点
  pinMode(MIN_Z, INPUT_PULLUP); // Z轴最小行程限位，同时也是归零点
  pinMode(MAX_X, INPUT_PULLUP); // X轴最大行程限位
  pinMode(MAX_Y, INPUT_PULLUP); // Y轴最大行程限位
  pinMode(MAX_Z, INPUT_PULLUP); // Z轴最大行程限位

  pinMode(x_dir, OUTPUT); // 配置方向控制引脚为输出引脚
  pinMode(y_dir, OUTPUT); // 配置方向控制引脚为输出引脚
  pinMode(z_dir, OUTPUT); // 配置方向控制引脚为输出引脚

  pinMode(x_pul, OUTPUT); // 配置脉冲控制引脚为输出引脚
  pinMode(y_pul, OUTPUT); // 配置脉冲控制引脚为输出引脚
  pinMode(z_pul, OUTPUT); // 配置脉冲控制引脚为输出引脚

  //-----------------------------
  engine.init();
  x_stepper = engine.stepperConnectToPin(x_pul);
  y_stepper = engine.stepperConnectToPin(y_pul);
  z_stepper = engine.stepperConnectToPin(z_pul);
  if (x_stepper)
  {
    x_stepper->setDirectionPin(x_dir);
    x_stepper->setSpeedInUs(MIN_STEP_uS); // the parameter is us/step !!!
    x_stepper->setAcceleration(X_ACCELERATE);
  }
  if (y_stepper)
  {
    y_stepper->setDirectionPin(y_dir);
    y_stepper->setSpeedInUs(MIN_STEP_uS); // the parameter is us/step !!!
    y_stepper->setAcceleration(Y_ACCELERATE);
  }
  if (z_stepper)
  {
    z_stepper->setDirectionPin(z_dir);
    z_stepper->setSpeedInUs(MIN_STEP_uS); // the parameter is us/step !!!
    z_stepper->setAcceleration(Z_ACCELERATE);
  }
}

void mov2xyz(int16_t x,int16_t y,int16_t z)
{

  x_stepper->move(map(x, -X_MAX_MOV, X_MAX_MOV, -2044, 2044 ) * 2);//根据计算的脉冲数与真实长度进行映射
  y_stepper->move(map(y, -Y_MAX_MOV, Y_MAX_MOV, -776, 776 ) * 10 );
  z_stepper->move(map(z, -Z_MAX_MOV, Z_MAX_MOV, -512, 512 ) * 10);

}


/********************************************************
* Function name: tsk_executer_ctrl
* Description  : mqtt 命令接收解析处理 线程函数
* Parameter    : pvParam 线程创建传入参数

                 命令说明：rssi=-1 进入OTA远程升级
                          rssi=0 远程重启
                          rssi为其他值，招测命令

* return         void
**********************************************************/

void tsk_executer_ctrl(void *pvParam)
{
  ctrl_msg_queue_t mqtt_ctrl_msg_rec; // 接收到的MOSQUITTO控制消息，包含key:读写标识 和 value:控制量

  steper_motor_init(); // fastaccelstepper步进电机驱动库初始化
  delay(3000);

  move2orgin_p();
  delay(3000);

  mov2xyz(X_MAX_MOV/2,Y_MAX_MOV/2,Z_MAX_MOV/2);
  delay(3000);

  // clt_x_num();
  // delay(3000);
  // clt_y_num();
  // delay(3000);
  // clt_z_num();
  while (1)
  {
    if (xQueueReceive(g_mqtt_ctrl_msgQ, &mqtt_ctrl_msg_rec, MQTT_UPLOAD_CYCLE * 1000) == pdTRUE)
    {
      Serial.println("MQTT--->ESP32: ctrl massage received!!!");


      if (String(mqtt_ctrl_msg_rec.cmd) == "STOP")
      { // 按照坐标运动
        z_stepper->stopMove();
      }
      if (String(mqtt_ctrl_msg_rec.cmd) == "ZERO")
      { // 按照坐标运动
        move2orgin_p();
      }
      if (String(mqtt_ctrl_msg_rec.cmd) == "RMOV")
      {
        Serial.printf("RMOV: (%d,%d,%d)\r\n",mqtt_ctrl_msg_rec.v1,mqtt_ctrl_msg_rec.v2,mqtt_ctrl_msg_rec.v3);
        x_stepper->move(map(mqtt_ctrl_msg_rec.v1, -X_MAX_MOV, X_MAX_MOV, -2044, 2044 ) * 2);//根据计算的脉冲数与真实长度进行映射
        y_stepper->move(map(mqtt_ctrl_msg_rec.v2, -Y_MAX_MOV, Y_MAX_MOV, -776, 776 ) * 10 );
        z_stepper->move(map(mqtt_ctrl_msg_rec.v3, -Z_MAX_MOV, Z_MAX_MOV, -512, 512 ) * 10);
      }

    } // endofif (xQueueReceive(g_mqtt_ctrl_msgQ,
      //-----------消息队列超时控制自主上报周期，消息队列收到立即上报-------
    // mqtt_upload_json();         //upload json mqtt data
  } // end of while(1)
}

/********************************************************
* Function name: NVS_load_dataPoint
* Description  : 从non-volatile storage（flash）中载入数据点，恢复复位前状态
* Parameter    :

* return         void
**********************************************************/

static void NVS_load_dataPoint(void)
{
  EEPROM.begin(EEPROM_SIZE); // initialize EEPROM with predefined size

  for (uint8_t i = 0; i < sizeof(dev_data_t); i++)
  {
    dataPoint_EPPROM_shadowReg[i] = EEPROM.read(i);
  }
  //--------RESTOR data point----------
  memcpy((void *)(&x_devdatapoint), dataPoint_EPPROM_shadowReg, sizeof(dev_data_t)); // 数据恢复到g_dev_data中
  Serial.println("EEPROM data restore finished");

  //--------RESTOR EXCUTOR STATE----------
}

/********************************************************
* Function name: NVS_save_dataPoint
* Description  : 将设备数据点存入EEPROM
* Parameter    :

* return         void
**********************************************************/

static void NVS_save_dataPoint(void)
{
  memcpy(dataPoint_EPPROM_shadowReg, (void *)(&x_devdatapoint), sizeof(dev_data_t)); // memcpy到缓存
  for (uint8_t i = 0; i < sizeof(dev_data_t); i++)
  {
    EEPROM.write(i, dataPoint_EPPROM_shadowReg[i]);
  }
  delay(10);
  EEPROM.commit();
  Serial.println("EEPROM data write compplete");
}

/********************************************************
* Function name: system_software_reset
* Description  : 将设备数据点存入EEPROM后，重启ESP32，解决WIFI 无法连接、MQTT 连接断掉等问题
* Parameter    :

* return         void
**********************************************************/
void system_software_reset(void)
{
  NVS_save_dataPoint(); // save datapoint to EEPROM
  delay(1000);
  ESP.restart(); // after wifi_connect_counter times of trying, restart ESP32
}

// void clt_x_num()
// {
//   x_stepper->forceStop(); // 计算X轴步数x，x*100；
//   Serial.println("start calculate x");
//   if (HIGH == digitalRead(MIN_X))
//   {
//     Serial.println("RESET X TO ORIGIN POINT BEGIN");
//     while (HIGH == digitalRead(MIN_X))
//     {
//       x_stepper->move(-2);
//       delay(2);
//     }
//   }
//   x_stepper->forceStop();
//   delay(1000);

//   int cnt_x = 0;
//   while (HIGH == digitalRead(MAX_X))
//   {
//     x_stepper->move(2);
//     delay(2);
//     cnt_x += 1;
//   }
//   x_stepper->forceStop();
//   Serial.print("X Count:");
//   Serial.println(cnt_x);
//   delay(1000);

//   while (HIGH == digitalRead(MIN_X))
//   {
//     x_stepper->move(-2);
//     delay(2);
//   }
//   x_stepper->forceStop();
//   delay(1000);
// }
// void clt_y_num()
// {
//   y_stepper->forceStop(); // 计算X周轴步数x，x*100；
//   Serial.println("start calculate y");
//   if (HIGH == digitalRead(MIN_Y))
//   {
//     Serial.println("RESET Y TO ORIGIN POINT BEGIN");
//     while (HIGH == digitalRead(MIN_Y))
//     {
//       y_stepper->move(-10);
//       delay(10);
//     }
//   }
//   y_stepper->forceStop();
//   delay(1000);

//   int cnt_x = 0;
//   while (HIGH == digitalRead(MAX_Y))
//   {
//     y_stepper->move(10);
//     delay(10);
//     cnt_x += 1;
//     Serial.print("Y Count:");
//     Serial.println(cnt_x);
//   }
//   y_stepper->forceStop();
//   delay(1000);

//   while (HIGH == digitalRead(MIN_Y))
//   {
//     y_stepper->move(-10);
//     delay(10);
//   }
//   y_stepper->forceStop();
//   delay(1000);
// }
// void clt_z_num()
// {
//   z_stepper->forceStop(); // 计算X周轴步数x，x*100；
//   Serial.println("start calculate z");
//   if (HIGH == digitalRead(MIN_Z))
//   {
//     Serial.println("RESET Z TO ORIGIN POINT BEGIN");
//     while (HIGH == digitalRead(MIN_Z))
//     {
//       z_stepper->move(-10);
//       delay(10);
//     }
//   }
//   z_stepper->forceStop();
//   delay(1000);
//   int cnt_x = 0;
//   while (HIGH == digitalRead(MAX_Z))
//   {
//     z_stepper->move(1 * 10);
//     delay(10);
//     cnt_x += 1;
//     Serial.print("Z Count:");
//     Serial.println(cnt_x);
//   }
//   z_stepper->forceStop();
//   delay(1000);

//   while (HIGH == digitalRead(MIN_Z))
//   {
//     z_stepper->move(-10);
//     delay(10);
//   }
//   z_stepper->forceStop();
// }

void move2orgin_p()
{
  if (HIGH == digitalRead(MIN_X))
  {
    Serial.println("RESET X TO ORIGIN POINT BEGIN");
    while (HIGH == digitalRead(MIN_X))
    {
      x_stepper->move(-5);
      delay(5);
    }
  }
  x_stepper->forceStop();
  delay(30);
  Serial.println("RESET X TO ORIGIN POINT SUCCESS");

  if (HIGH == digitalRead(MIN_Y))
  {
    Serial.println("RESET Y TO ORIGIN POINT BEGIN");
    while (HIGH == digitalRead(MIN_Y))
    {
      y_stepper->move(-5);
      delay(5);
    }
  }
  y_stepper->forceStop();
  delay(30);
  Serial.println("RESET Y TO ORIGIN POINT SUCCESS");

  if (HIGH == digitalRead(MIN_Z))
  {
    Serial.println("RESET Z TO ORIGIN POINT BEGIN");
    while (HIGH == digitalRead(MIN_Z))
    {
      z_stepper->move(-5);
      delay(5);
    }
  }
  z_stepper->forceStop();
  delay(30);
  Serial.println("RESET Z TO ORIGIN POINT SUCCESS");
}

//-------------------------------------------------------------------
void setup()
{

  Serial.begin(115200);


  if (!SPIFFS.begin(true))
  {
    Serial.println("SPIFFS分区出错！");
    return;
  }
  Serial.println("SPIFFS 文件系统初始化成功！ ");

  NVS_load_dataPoint(); // EEPROM load函数先调用，因此在其第一行调用EEPROM.begin(size)避免遗漏！！
  setup_wifi();         // 连接WIFI AP

  http_server_init();   //


  // OTA_webserver_init();
  //----------ipc 对象创建--------
  g_mqtt_ctrl_msgQ = xQueueCreate(5, sizeof(ctrl_msg_queue_t));                                                                                            // 创建控制消息队列， mqtt callback发送至executer执行器线程
  // xTaskCreateUniversal(tsk_mqtt_service, "MQTTServiceTsk", CONFIG_ARDUINO_LOOP_STACK_SIZE, NULL, 11, &mqttTaskHandle, CONFIG_ARDUINO_RUNNING_CORE);        // modbus任务，注意堆栈设置为8K
  xTaskCreateUniversal(tsk_executer_ctrl, "MQTTMSGCmdEXE", CONFIG_ARDUINO_LOOP_STACK_SIZE, NULL, 12, &mqttExcutorTaskHandle, CONFIG_ARDUINO_RUNNING_CORE); // modbus任务，注意堆栈设置为8K
}

//-------------------------------------------------------------------
void loop()
{
  delay(3 * 1000);
}
